
#include "Interpolation.h"

////////////////////////////////////////////////////////////////////////////////
/////////////////////////PRIVATE METHODS////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
void Interpolation::CubicSplineInterpolation1D( )
{
   //perform cubic spline interpolation
   //assumes that the C-spline is a natural spline with S0=0 and Sn=0
   //the equation of the C-spline is of the form
   //gi(x) = ai(x-xi)^3 + bi(x-xi)^2 + ci(x-xi) + di
   //i represents the ith segment of the spline of n+1 points
   
   if ( DBG_INTERP > 1 )
      cout<<"Performing Cubic spline interpolation..."<<endl;
   if ( numPoints < 3 )
   {
      
      cout<<"Cannot perform C-spline interpolation with < 3 points"<<endl;
      exit(-1);
   }
   //if numPoints = n+1, then there are n equations to be solved since it is
   //assumed that it is a natural spline with S0=0 and Sn=0
   double him1, hi; //h_(i-1) and h_(i)
   double tmprhs;
   //allocate memory accordingly, numPoints-2 = n-1
   gsl_vector* ld = gsl_vector_calloc( numPoints-2 );
   gsl_vector* ud = gsl_vector_calloc( numPoints-2 );
   gsl_vector* md = gsl_vector_calloc( numPoints-2 );
   gsl_vector* rhs = gsl_vector_calloc( numPoints-2 );
   gsl_vector* S = gsl_vector_calloc( numPoints );
   int j;
   for ( int i=1;i<numPoints-1;i++ )//1 to n-1 = n-1 equations
   {
      j = i-1;
      him1 = x.at(i) - x.at(i-1);
      hi = x.at(i+1) - x.at(i);
      //set lower diagonal elements
      if ( i==1 )
         gsl_vector_set( ld, j, 0 );
      else
         gsl_vector_set( ld, j, him1 );
      //set upper diagonal emelents
      if ( i==(numPoints-2) )
         gsl_vector_set( ud, j, 0 );
      else
         gsl_vector_set( ud, j, hi );
      //set main diagonal elements
      gsl_vector_set( md, j, 2.0*(him1+hi) );
      //set RHS
      tmprhs = 6.0*( (y.at(i+1)-y.at(i))/hi - (y.at(i)-y.at(i-1))/him1 );
      gsl_vector_set( rhs, j, tmprhs );
   }
   //test DBG
   if ( DBG_INTERP > 1 )
   {
      for ( size_t j=0;j<ld->size;j++ )
         cout<<"  lower diagonal elements:"<<j<<" "<<
               gsl_vector_get( ld, j )<<endl;
      for ( size_t j=0;j<ud->size;j++ )
         cout<<"  upper diagonal elements:"<<j<<" "<<
               gsl_vector_get( ud, j )<<endl;
      for ( size_t j=0;j<md->size;j++ )
         cout<<"  main diagonal elements:"<<j<<" "<<
               gsl_vector_get( md, j )<<endl;
      for ( size_t j=0;j<rhs->size;j++ )
         cout<<"  RHS elements:"<<j<<" "<<
               gsl_vector_get( rhs, j )<<endl;
   }
   //now solve the tri-diagonal matrix to find values of S
   gsl_vector* tmpVctr;
   tdma* ptr_tdma = new tdma( );
   ptr_tdma->Solve( ld, md, ud, rhs );
   tmpVctr = ptr_tdma->GetX( );
   for ( int i=1;i<numPoints-1;i++ )
      gsl_vector_set( S, i, gsl_vector_get(tmpVctr,i-1) );
   gsl_vector_set( S, 0, 0 );
   gsl_vector_set( S, numPoints-1, 0 );
   
   //test DBG
   if ( DBG_INTERP > 1 )
   {
      for ( size_t j=0;j<S->size;j++ )
         cout<<"  S elements:"<<j<<" "<<gsl_vector_get( S, j )<<endl;
   }
   //now compute a, b, c, and d (the coefficients) using the S values
   double tmp;
   for ( int i=0;i<numPoints-1;i++ )
   {
      tmp = ( gsl_vector_get( S, i+1 ) - 
            gsl_vector_get( S, i ) )/( 6.0*(x.at(i+1) - x.at(i)) );
      gsl_vector_set( a, i, tmp );
      tmp = gsl_vector_get( S, i )/(double)(2);
      gsl_vector_set( b, i, tmp );
      tmp = ( y.at(i+1) - y.at(i) )/( x.at(i+1) - x.at(i) ) - 
            ( 2.0*( x.at(i+1) - x.at(i) )*gsl_vector_get( S, i ) + 
            ( x.at(i+1) - x.at(i) )*gsl_vector_get( S, i+1 ) )/6.0;
      gsl_vector_set( c, i, tmp );
      gsl_vector_set( d, i, y.at(i) );
   }
   //test DBG
   /*for ( int i=0;i<a.size();i++ )
      cout<<a.at(i)<<"\t"<<b.at(i)<<"\t"<<c.at(i)<<"\t"<<d.at(i)<<endl;*/
   //clean up
   gsl_vector_free( ld );
   gsl_vector_free( ud );
   gsl_vector_free( md );
   gsl_vector_free( rhs );
   gsl_vector_free( S );
   if ( DBG_INTERP > 1 )
      cout<<"Done"<<endl;
}
////////////////////////////////////////////////////////////////////////////////
int Interpolation::GetPosition( double xat )
{
   //returns the integer position where xat exists in x vector
   //find the range in which x exists
   size_t position = 0;
   if ( ( xat < x.at(0) ) || ( xat > x.at(x.size()-1 ) ) )
   {
      cout<<"------>>>x val "<<
            xat<<" not in range...please check!!!!!!!!"<<endl;
      exit( -1 );
   }
   else
   {
      for ( size_t i=0;i<x.size()-1;i++ )
      {
         if ( ( xat >= x.at(i) ) && ( xat<=x.at(i+1) ) )
         {
             position = i;
             break;
         }
      }
   }
   if ( DBG_INTERP > 1 )
   {
      cout<<"Position "<<xat<<" found at :"<<position<<endl;
      cout<<"Left val :"<<x.at(position)<<" Right val :"<<x.at(position+1)
            <<endl;
   }
   
   return int( position );
}
////////////////////////////////////////////////////////////////////////////////
int Interpolation::GetPosition( double xat, vector<double> inpData )
{
   //returns the integer position where xat exists in x vector
   //find the range in which x exists
   size_t position = 0;
   if ( ( xat < inpData.at(0) ) || ( xat > inpData.at(inpData.size()-1 ) ) )
   {
      cout<<"------>>>x val "<<
            xat<<" not in range...please check!!!!!!!!"<<endl;
      exit( -1 );
   }
   else
   {
      for ( size_t i=0;i<inpData.size()-1;i++ )
      {
         if ( ( xat >= inpData.at(i) ) && ( xat<=inpData.at(i+1) ) )
         {
             position = i;
             break;
         }
      }
   }
   if ( DBG_INTERP > 1 )
   {
      cout<<"Position "<<xat<<" found at :"<<position<<endl;
      cout<<"Left val :"<<inpData.at(position)<<" Right val :"<<
            inpData.at(position+1)<<endl;
   }
   
   return int( position );
}
////////////////////////////////////////////////////////////////////////////////
void Interpolation::LinearInterpolation( )
{
   //implementation of piece-wise linear interpolation to compute coefficients
   //y=ax+b
   //a set of coefficients a and b are computed for each interval in data
   double tmpa, tmpb;
   if ( DBG_INTERP > 1 )
      cout<<"Piecewise linear curve coefficients "<<endl;

   for ( int i=0;i<(numPoints-1);i++ )
   {
      tmpa = ( y.at(i+1) - y.at(i) )/( x.at(i+1) - x.at(i) );
      gsl_vector_set( a, i, tmpa );
      tmpb = y.at(i) - tmpa*x.at(i);
      gsl_vector_set( b, i, tmpb );
      if ( DBG_INTERP > 1 )
      {
         cout<<i<<"\t"<<tmpa<<"\t"<<tmpb<<endl;
      }
   }
}
////////////////////////////////////////////////////////////////////////////////
////////////////////////PUBLIC METHODS//////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

Interpolation::Interpolation( )
{
   //default constructor
}

////////////////////////////////////////////////////////////////////////////////
Interpolation::Interpolation( vector<double> x, vector<double> y )
{
   //overloaded constructor
   this->x = x;
   this->y = y;
   
   if ( x.size() != y.size() )
      cout<<"Please check input vectors, number of params should be equal!"<<
            endl;
   //sort data so that everything is in order
   stable_sort( x.begin(), x.end() );
   //set number of points
   numPoints = (int)x.size( );
   //allocate memory for coefficients
   a = gsl_vector_calloc( numPoints-1 );
   b = gsl_vector_calloc( numPoints-1 );
   c = gsl_vector_calloc( numPoints-1 );
   d = gsl_vector_calloc( numPoints-1 );
   //test DBG
   if ( DBG_INTERP > 1 )
   {
      cout<<"x"<<"\t y"<<endl;
      for ( size_t i=0;i<(unsigned)numPoints;i++ )
         cout<<x.at(i)<<"\t"<<y.at(i)<<endl;
   }
}
////////////////////////////////////////////////////////////////////////////////
Interpolation::~Interpolation( )
{
   gsl_vector_free( a );
   gsl_vector_free( b );
   gsl_vector_free( c );
   
   gsl_vector_free( d );
}

////////////////////////////////////////////////////////////////////////////////
void Interpolation::SetInputPoints1D( vector<double> xin, vector<double> yin )
{
   //set input points
   this->x = xin;
   this->y = yin;
   if ( x.size() != y.size() )
      cout<<"Please check input vectors, number of params should be equal!"<<
            endl;
   //sort data so that everything is in order
   stable_sort( x.begin(), x.end() );
   //set number of points
   numPoints = (int)x.size( );
   //allocate memory for coefficients
   a = gsl_vector_calloc( numPoints-1 );
   b = gsl_vector_calloc( numPoints-1 );
   c = gsl_vector_calloc( numPoints-1 );
   
   d = gsl_vector_calloc( numPoints-1 );
   if ( DBG_INTERP > 1 )
   {
      cout<<"x"<<"\t y"<<endl;
      for ( size_t i=0;i<(unsigned)numPoints;i++ )
         cout<<x.at(i)<<"\t"<<y.at(i)<<endl;
   }
}
////////////////////////////////////////////////////////////////////////////////
vector < gsl_vector* >  Interpolation::GetCubicSplineCoefficientVectors( )
{
   
   this->CubicSplineInterpolation1D( );
   cSplineCoeffs.push_back( a );
   cSplineCoeffs.push_back( b );
   cSplineCoeffs.push_back( c );
   cSplineCoeffs.push_back( d );
   
   return cSplineCoeffs;
}


////////////////////////////////////////////////////////////////////////////////
double Interpolation::EvaluateCubicSplineAt( double xat )
{
   this->CubicSplineInterpolation1D( );
   cSplineCoeffs.push_back( a );
   cSplineCoeffs.push_back( b );
   cSplineCoeffs.push_back( c );
   cSplineCoeffs.push_back( d );

   double yat = 0;
   //y = a(x-xat)^3 + b(x-xat)^2 + c(x-xat) + d
   int evalAt;
   evalAt = this->GetPosition( xat );
   if ( DBG_INTERP > 1 )
      cout<<"Eval at :"<<evalAt<<endl;
   yat = gsl_vector_get(a, evalAt)*pow((xat-x.at(evalAt)),3.0) + 
            gsl_vector_get(b, evalAt)*pow((xat-x.at(evalAt)),2.0) + 
            gsl_vector_get(c, evalAt)*(xat-x.at(evalAt)) + 
         
            gsl_vector_get(d, evalAt);
   
   return yat;
}
////////////////////////////////////////////////////////////////////////////////
double Interpolation::EvaluateCubicSplineAt( double xat, vector<double> xdata )
{
   
   this->CubicSplineInterpolation1D( );
   cSplineCoeffs.push_back( a );
   cSplineCoeffs.push_back( b );
   cSplineCoeffs.push_back( c );
   cSplineCoeffs.push_back( d );

   double yat = 0;
   //y = a(x-xat)^3 + b(x-xat)^2 + c(x-xat) + d
   int evalAt;
   evalAt = this->GetPosition( xat );
   if ( DBG_INTERP > 1 )
      cout<<"Eval at :"<<evalAt<<endl;
   yat = gsl_vector_get(a, evalAt)*pow((xat-xdata.at(evalAt)),3.0) + 
            gsl_vector_get(b, evalAt)*pow((xat-xdata.at(evalAt)),2.0) + 
            gsl_vector_get(c, evalAt)*(xat-xdata.at(evalAt)) + 
         
            gsl_vector_get(d, evalAt);
   
   return yat;
}

////////////////////////////////////////////////////////////////////////////////
void Interpolation::SetInputPoints( map< int, vector< double > > inp, 
            gsl_matrix* out )
{
   //the vector of vectors represents each set of input parameters
   //for the n-dimensional splines
   /*if ( DBG_INTERP )
   {
      cout<<"Number of parameters is :"<<inp.size()<<endl;
      cout<<"Number of outputs is :"<<(out->size1)*(out->size2)<<endl;
   }
   //fit splines in x direction
   vector<double> ydata;
   vector<double> xdata = inp.find(0)->second;
   ydata.resize( xdata.size() );
   cout<<"Data being fit :"<<endl;
   for ( int i=0;i<xdata.size();i++ )
   {
      ydata.at(i) = gsl_matrix_get( out, i, 0 );
      cout<<xdata.at(i)<<"\t"<<gsl_matrix_get( out, i, 0 )<<endl;
   }
   this->SetInputPoints1D( xdata, ydata );*/
   
}
////////////////////////////////////////////////////////////////////////////////
double Interpolation::EvaluateLinearAt( double xat )
{
   //evaluation of linear interpolation data at xat
   return -1.0f;
}
////////////////////////////////////////////////////////////////////////////////
double Interpolation::EvaluateLinearAt( double xat, vector<double> xdata )
{
   
   //evaluation of linear interpolation data at xat
   this->LinearInterpolation( );   
   lineCoeffs.push_back( a );
   lineCoeffs.push_back( b );

   double yat = 0;
   //y = ax+b
   int evalAt;
   evalAt = this->GetPosition( xat, xdata );
   if ( DBG_INTERP > 1 )
      cout<<"Eval at :"<<evalAt<<endl;
   yat = gsl_vector_get(a, evalAt)*(xat) + gsl_vector_get(b, evalAt);
   
   return yat;
}
////////////////////////////////////////////////////////////////////////////////
/*vector< gsl_vector* > Interpolation::GetLinearCoefficientVectors( )
{
   this->LinearInterpolation( );
   lineCoeffs.push_back( a );
   lineCoeffs.push_back( b );
    return
}*/
